/**
  ******************************************************************************
  * @file    can_open.c
  * @author  Qilong Huang
  * @version V0.0.1
  * @date    10-03-2020
  * @brief   This file contains CANopen specific functions.  
  *          
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; KION(SZ) R&D Center
  *
  * Licensed under KION, (the "License");
  *
  ******************************************************************************
  */ 
/* Includes ------------------------------------------------------------------*/
#include "CAN_canopen.h"
#include "stdlib.h"
#include "string.h"
#include "Hourmeter.h"

/* Private function declaration ----------------------------------------------*/ 
static int setup_canopen_nmt(void);
static int setup_canopen_sdo(void);
static int setup_canopen_cal(void);
static int setup_kwp(void);
static int setup_canopen_pdo(void);
static int setup_canopen_tx_heartbeat(void);
static int setup_canopen_emergency(void);
static s16 set_nmt_state(s16 command_specifier);
static void process_canopen_nmt(CanRxMsg* RxMessage);
static void process_kwp_mosi(CanRxMsg* RxMessage);

static void send_kwp_response(u32 cob_id);

static u32 secacc_crypto_12( u32 seed );
static u32 secacc_crypto_34( u32 seed);
static u32 secacc_crypto_56( u32 seed);
enum kwp_response_code secacc_process_request( u8 condition, u8  *dat, u16 *len);
static u32 secacc_fortuna( void );

static void process_sync_msg(CanRxMsg* RxMessage);

static void process_br1189_lift_pdo1_rx(CanRxMsg* RxMessage);
static void process_br1189_lift_pdo3_rx(CanRxMsg* RxMessage);
static void process_br1189_trac_pdo1_rx(CanRxMsg* RxMessage);
static void process_br1189_trac_pdo3_rx(CanRxMsg* RxMessage);
static void process_br1189_trac_pdo5_rx(CanRxMsg* RxMessage);
static void process_br1189_steer_pdo1_rx(CanRxMsg* RxMessage);
static void process_br1189_steer_pdo3_rx(CanRxMsg* RxMessage);
void FillSDTCTxBuf(E_STORED_FAULT_PARAM *stored_fault, u16 total_fault, u16 device);
void CalTotalStoredFault(E_STORED_FAULT_PARAM *stored_fault, u16 *total_fault);
void can_send_heartbeat(u8 can_status);
void can_send_heartbeat_operational(void);
/********************************************************************************************************
*Local variables
********************************************************************************************************/
//static u16 heartbeat_identifier = 0;
static enum secacc_state secacc_security_state;
static u32 secacc_code;
static u8 secacc_expected_service;
/********************************************************************************************************
*Global variables
********************************************************************************************************/
s16 canopen_heart_beat_rate = 0;
s16 canopen_emergency_rate = 0;
s16 can_pdo_timeout = 0;
s16 can_pdo_timeout_period = 3000;

s16 can_canopen_system_status_flags = 0;
s16 can_nmt_state = 0;
// Working Periodic Service Rates
s16 canopen_heart_beat_timer = 0;
s16 canopen_emergency_timer = 0;
u16 br1189_steer_timeout;
u16 br1189_traction_timeout;
u16 br1189_lift_timeout;
u8 can_pdo_data[8];
union CAN_SYSTEM_FLAGS CanSystemFlags = {0};

CanTxMsg d_txMsg[E_TX_BUFF_SIZE] = {0};
u8	can_tx_head = 0;
u8	can_tx_tail = 0;

u32 kwp_cob_id;
static u8 kwp_multi_frame_send = 0;
static u8 kwp_multi_frame = 0;
static u8 kwp_seq_no = 0;
static s16 kwp_data_length = 0;
static s16 kwp_data_index = 0;
static u16 kwp_rx_data_length = 0;
static s16 kwp_object_id;
static s16 kwp_object_sub_id;
static s16 kwp_var_index;

static u8 kwp_blk_size = 0;
static s8 kwp_multi_frame_timeout = 0;
static u8  *mulit_char_ptr = 0; 


s8 kwp_needs_ee;
u8 kwp_ee_index;

E_STORED_FAULT_PARAM br1347_traction_stored_fault[MAX_STORED_FAULT] = {0};
E_STORED_FAULT_PARAM br1347_lift_stored_fault[MAX_STORED_FAULT] = {0};
E_STORED_FAULT_PARAM br1275_bms_stored_fault[MAX_STORED_FAULT] = {0};
E_STORED_FAULT_PARAM rfid_stored_fault[MAX_STORED_FAULT] = {0};

static struct fortune fortune_fate;
Lift_PDO1_Type BR1189_lift_status;
Steer_PDO1_Type BR1189_steer_status;
Trac_PDO1_Type BR1189_trac_status;
#define DEADDROP_RAM_ADDR   0x2000BFF0U
#define KWP_FLASH_FLAG     0xAAFF0055

u8 dtc_error_tx_buf[320];
u16 display_total_stored_fault;

u8 test_can_buff[8] = {0};
u8 test_can_transmit_buf[8] = {0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77};
/*CAN Filter number*/
/*
KWP---CANOPEN_FILTER_NUMBER_0, CANOPEN_FILTER_NUMBER_1, CANOPEN_FILTER_NUMBER_2
MNT---CANOPEN_FILTER_NUMBER_3
PDO---CANOPEN_FILTER_NUMBER_4 ~ CANOPEN_FILTER_NUMBER_9
*/
/**
* @brief	setup for nmt
* @param	none
* @note     none
* @retval   int
*/
static int setup_canopen_nmt(void)
{
    u16 temp_cob_id = 0;
    CAN_FilterInitTypeDef  CAN_FilterInitStructure = {0};
    
    temp_cob_id = make_cob_id(NMT, 0, 0);

    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_3;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;

    CAN_FilterInit(&CAN_FilterInitStructure);

    return(1);
}


/**
* @brief	setup for sdo
* @param	none
* @note     none
* @retval   int
*/
static int setup_canopen_sdo(void)
{
    return(1);    

}

/**
* @brief	setup for canopen cal
* @param	none
* @note     none
* @retval   int
*/
static int setup_canopen_cal(void)
{
    return(1);    
}

/**
* @brief	setup for kwp2000
* @param	none
* @note     none
* @retval   int
*/
static int setup_kwp(void)
{
    u16 temp_cob_id = 0;
    CAN_FilterInitTypeDef  CAN_FilterInitStructure = {0};
    
    temp_cob_id = DISP_KWP_RX_ID;
    
    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_0;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);

    temp_cob_id = DISP_KWP_RX_ID_ALL;
    
    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_1;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);

    return(1);
}

/**
* @brief	setup for pdo
* @param	none
* @note     none
* @retval   int
*/
static int setup_canopen_pdo(void)
{
    u16 temp_cob_id = 0;

    CAN_FilterInitTypeDef  CAN_FilterInitStructure = {0};

    temp_cob_id = BR1189_STEER_PDO1_RX_ID;  // 0x183

    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_4;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);

    temp_cob_id = BR1189_STEER_PDO3_RX_ID;  // 0x383

    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_5;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
	
    temp_cob_id = BR1189_TRAC_PDO1_RX_ID;  // 0x181

    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_6;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
	
	temp_cob_id = BR1189_TRAC_PDO3_RX_ID;  // 0x381
	/* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_7;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
	
    temp_cob_id = BR1189_LIFT_PDO1_RX_ID;  // 0x182

    /* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_8;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
	
	temp_cob_id = BR1189_LIFT_PDO3_RX_ID;  // 0x382
	/* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_9;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
    
    temp_cob_id = BR1189_TRAC_PDO5_RX_ID1;  // 0x304
	/* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_10;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
    
    temp_cob_id = BR1189_TRAC_PDO5_RX_ID2;  // 0x581
	/* CAN filter init */
    CAN_FilterInitStructure.CAN_FilterNumber = CANOPEN_FILTER_NUMBER_11;
    CAN_FilterInitStructure.CAN_FilterMode = CAN_FilterMode_IdMask;
    CAN_FilterInitStructure.CAN_FilterScale = CAN_FilterScale_32bit;
    CAN_FilterInitStructure.CAN_FilterIdHigh = (temp_cob_id & FILTER_MASK) << SHIFT_5_BITS;
    CAN_FilterInitStructure.CAN_FilterIdLow = 0;
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh = STD_MASK;
    CAN_FilterInitStructure.CAN_FilterMaskIdLow = 0x0000;
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment = CAN_FIFO0;
    CAN_FilterInitStructure.CAN_FilterActivation = ENABLE;
    CAN_FilterInit(&CAN_FilterInitStructure);
    return(1);
}

/**
* @brief	setup for heartbeat
* @param	none
* @note     none
* @retval   none
*/
static int setup_canopen_tx_heartbeat(void)
{
//    heartbeat_identifier = make_cob_id(HEARTBEAT, 0, working_can_slave_id);

    if ((canopen_heart_beat_rate > 250) || (canopen_heart_beat_rate < 4))
    {
        canopen_heart_beat_rate = 5;
    }
    canopen_heart_beat_timer = 1;

    can_canopen_system_status_flags = can_canopen_system_status_flags | CAN_OPEN_HEARTBEAT_ENABLED;

    return(1);
}

/**
* @brief	setup for emergency type
* @param	none
* @note     none
* @retval   none
*/
static int setup_canopen_emergency(void)
{
    
    if ( (canopen_emergency_rate > 250) || (canopen_emergency_rate < 4) )  
    {
        canopen_emergency_rate = 4;
    }
    
    canopen_emergency_timer = canopen_emergency_rate;

    can_canopen_system_status_flags = can_canopen_system_status_flags | CAN_OPEN_EMERGENCY_ENABLED;

    return(1);
}


/**
* @brief	canopen_init
* @param	none
* @note     none
* @retval   int
*/
int canopen_init(void)
{
    u8 result = 1;
    result = setup_canopen_nmt();
    if(result == 0)
    {
        return(0);
    }
    result = setup_canopen_sdo();
    if(result == 0) 
    {
        return(0);
    }
    result = setup_canopen_cal();
    if(result == 0) 
    {
        return(0);
    }
    result = setup_kwp();
    if(result == 0) 
    {
        return(0);
    }
    result = setup_canopen_pdo();
    if(result == 0) 
    {
        return(0);
    }
    result = setup_canopen_tx_heartbeat();
    if(result == 0) 
    {
        return(0);
    }
    result = setup_canopen_emergency();
    if (result == 0)
    {
        return(0);
    }   
    can_nmt_state = NMT_INITIALIZATION;
    can_canopen_system_status_flags = can_canopen_system_status_flags | CAN2_CANOPEN_ENABLED;

    can_tx_head = 0;
    can_tx_tail = 0;

    return(1);
}

/**
* @brief	set nmt state
* @param	s16 command_specifier
* @note     none
* @retval   none
*/
s16 set_nmt_state(s16 command_specifier)
{
    switch(command_specifier) 
    {
    case NMT_CS_RESET_NODE:
        can_nmt_state = NMT_INITIALIZATION;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 0;
        CAN_PDO_Timed_Out = 0;
        CAN_Enabled = 0;
//	    reset_controller();                                                ---Modify
        break;

    case NMT_CS_RESET_COMMUNICATION:
        can_nmt_state = NMT_PRE_OPERATIONAL;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 1;
        CAN_PDO_Timed_Out = 0;
        CAN_Config();
        break;

    case NMT_CS_STOP_REMOTE_NODE:
        can_nmt_state = NMT_STOPPED;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 0;
        CAN_PDO_Timed_Out = 0;
        break;

    case NMT_CS_ENTER_PRE_OPERATIONAL:
        can_nmt_state = NMT_PRE_OPERATIONAL;
        CAN_PDO_Enabled = 0;
        CAN_PDO_Timeouts_On = 0;
        can_pdo_timeout = can_pdo_timeout_period;
        CAN_SDO_Enabled = 1;
        break;

    case NMT_CS_START_REMOTE_NODE:
        can_nmt_state = NMT_OPERATIONAL;
        CAN_PDO_Enabled = 1;
        CAN_SDO_Enabled = 1;
        CAN_PDO_Timed_Out = 0;
    	CAN_Time_Out = 0;
    	break;

    default:
        return 0;
    }
    return 1;
}

/**
* @brief	process CANopen nmt message
* @param	CanRxMsg* RxMessage: points to message
* @note     none
* @retval   none
*/
void process_canopen_nmt(CanRxMsg* RxMessage)
{
    s16 node_id = 0;
    s16 temp_node_id = 0;

    if(RxMessage == 0)
    {
        return;
    }

    node_id = RxMessage->Data[1];

    if(node_id != 0) 
    {
        temp_node_id = working_can_slave_id;
        if(temp_node_id != node_id)
        {
            return;
        }
    }
    set_nmt_state(RxMessage->Data[0]);
}
u8 kk;
/**
* @brief	Process BR1189 traction PDO1 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_lift_pdo1_rx(CanRxMsg* RxMessage)
{
    if(RxMessage == NULL || CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_lift_timeout = 0;
    /*Byte[6]: LAC-lift Status*/
	/*Bit7: lift hour counting require.     0-sleep, 1-counting request*/
	/*Bit6: Pump oil level.                 0-ok,    1-oil lack*/
	/*Bit5: Lift motor over temperature.    0-ok,    1-over temperature*/
	/*Bit4: Lift FETs over temperature      0-ok,    1-over temperature*/
	/*Bit3-Bit0:Lift PDO1 counter*/
	/*Byte[7]: Checksum*/
	if(crc8_compute_bit(can_pdo_data, sizeof(can_pdo_data), crc8_init) == 0)
	{
        if((can_pdo_data[6] & BIT7) == BIT7)
		{
		    BR1189_lift_status.hourmeter_counter_flag = HM_COUNTER_ON;
		}
		else
		{
		    BR1189_lift_status.hourmeter_counter_flag = HM_COUNTER_OFF;
		}
        if((can_pdo_data[6] & BIT6) == BIT6)
		{
		    BR1189_lift_status.pump_oil_level = OIL_LACK;
		}
		else
		{
		    BR1189_lift_status.pump_oil_level = OIL_OK;
		}
        if((can_pdo_data[6] & BIT5) == BIT5)
		{
		    BR1189_lift_status.motor_over_tempurate = TEMPURATE_OVER;
		}
		else
		{
		    BR1189_lift_status.motor_over_tempurate = TEMPURATE_OK;
		}
        if((can_pdo_data[6] & BIT4) == BIT4)
		{
		    BR1189_lift_status.fets_over_tempurate = TEMPURATE_OVER;
		}
		else
		{
		    BR1189_lift_status.fets_over_tempurate = TEMPURATE_OK;
		}
	}
}

/**
* @brief	Process BR1189 traction PDO1 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_lift_pdo3_rx(CanRxMsg* RxMessage)
{
    if(RxMessage == NULL || CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_lift_timeout = 0;

    Disp_LiftErrCode[0] = (can_pdo_data[1] << 8) | can_pdo_data[0];
	Disp_LiftErrCode[1] = (can_pdo_data[3] << 8) | can_pdo_data[2];
	Disp_LiftErrCode[2] = (can_pdo_data[5] << 8)| can_pdo_data[4];
	Disp_LiftErrCode[3] = (can_pdo_data[7] << 8)| can_pdo_data[6];
}
/**
* @brief	Process BR1189 traction PDO1 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_trac_pdo1_rx(CanRxMsg* RxMessage)
{
    U8 i;
    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_traction_timeout = 0;
    for(i = 0; i < 8; i ++)
    {
        test_can_buff[i] = can_pdo_data[i];
    }
    
}

/**
* @brief	Process BR1189 traction PDO1 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_trac_pdo3_rx(CanRxMsg* RxMessage)
{
    if(RxMessage == NULL || CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_traction_timeout = 0;

    Disp_TracErrCode[0] = (can_pdo_data[1] << 8) | can_pdo_data[0];
	Disp_TracErrCode[1] = (can_pdo_data[3] << 8) | can_pdo_data[2];
	Disp_TracErrCode[2] = (can_pdo_data[5] << 8)| can_pdo_data[4];
	Disp_TracErrCode[3] = (can_pdo_data[7] << 8)| can_pdo_data[6];    
}
/**
* @brief	Process BR1189 traction PDO5 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_trac_pdo5_rx(CanRxMsg* RxMessage)
{
    if(RxMessage == NULL || CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_traction_timeout = 0;
    if((can_pdo_data[4] & BIT5) == BIT5)
    {
        Battary_type = Li_Bat;
        Li_BattCon = can_pdo_data[6];
    }
    else
    {
        Battary_type = Lead_Bat;
    }
}
/**
* @brief	Process BR1189 traction PDO1 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_steer_pdo1_rx(CanRxMsg* RxMessage)
{
    if(RxMessage == NULL || CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_steer_timeout = 0;
	
	if(crc8_compute_bit(can_pdo_data, sizeof(can_pdo_data), crc8_init) == 0)
	{
        BR1189_steer_status.steer_wheel_angle = (can_pdo_data[1] << 8) | can_pdo_data[0];
		BR1189_steer_status.steer_speed_status = can_pdo_data[2];
		if((can_pdo_data[6] & BIT7) == BIT7)
		{
		    BR1189_steer_status.hourmeter_counter_flag = HM_COUNTER_ON;
		}
		else
		{
		    BR1189_steer_status.hourmeter_counter_flag = HM_COUNTER_OFF;
		}
		if((can_pdo_data[6] & BIT6) == BIT6)
		{
		    BR1189_steer_status.motor_over_tempurate = TEMPURATE_OVER;
		}
		else
		{
		    BR1189_steer_status.motor_over_tempurate = TEMPURATE_OK;
		}
		if((can_pdo_data[6] & BIT5) == BIT5)
		{
		    BR1189_steer_status.fets_over_tempurate = TEMPURATE_OVER;
		}
		else
		{
		    BR1189_steer_status.fets_over_tempurate = TEMPURATE_OK;
		}
	}
}

/**
* @brief	Process BR1189 traction PDO1 message
* @param	CanRxMsg* RxMessage points to received msssage
* @note     none 
* @retval   none
*/
static void process_br1189_steer_pdo3_rx(CanRxMsg* RxMessage)
{
    if(RxMessage == NULL || CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    br1189_steer_timeout = 0;

    Disp_SteerErrCode[0] = (can_pdo_data[1] << 8) | can_pdo_data[0];
	Disp_SteerErrCode[1] = (can_pdo_data[3] << 8) | can_pdo_data[2];
	Disp_SteerErrCode[2] = (can_pdo_data[5] << 8)| can_pdo_data[4];
	Disp_SteerErrCode[3] = (can_pdo_data[7] << 8)| can_pdo_data[6];    
}

/**
* @brief	process send time
* @param	CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void can_send_timestamp(void)
{
	union time_farmat
	{
        u32 timearry;
		u8 can_data[4];
	} can_time_farmat;
    CanTxMsg txMsg = {0};
	can_time_farmat.timearry = 0;
    u8 year_temp = sys_time.t_year - 2000;
    u8 mounth_temp = sys_time.t_mounth;
	u8 date_temp = sys_time.t_date;
	u8 hour_temp = sys_time.t_hour;
	u8 min_temp = sys_time.t_min;
	u8 sec_temp = sys_time.t_sec;
	
	can_time_farmat.timearry |= date_temp << 27;
	can_time_farmat.timearry |= mounth_temp << 23;
	can_time_farmat.timearry |= year_temp << 17;
	can_time_farmat.timearry |= hour_temp << 12;
	can_time_farmat.timearry |= min_temp << 6;
	can_time_farmat.timearry |= sec_temp;
	
    txMsg.StdId = 0x184; 
      
	txMsg.Data[0] = Hrm_Struct.HM3_value ;
    txMsg.Data[1] = Hrm_Struct.HM3_value >> 8;
    txMsg.Data[2] = can_time_farmat.can_data[0];
    txMsg.Data[3] = can_time_farmat.can_data[1];
    txMsg.Data[4] = can_time_farmat.can_data[2];
    txMsg.Data[5] = can_time_farmat.can_data[3];
    txMsg.Data[6] = Current_Bat_Soc;
    txMsg.Data[7] = 0;
    
    txMsg.IDE = CAN_STD_ID;
    txMsg.DLC = 8;
    CAN_set_tx_msg_to_buff(&txMsg);
}
/**
* @brief	process send time
* @param	CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void can_send_heartbeat(u8 can_status)
{
    CanTxMsg txMsg = {0};
    txMsg.StdId = 0x704; 
      
	txMsg.Data[0] = can_status;
    
    txMsg.IDE = CAN_STD_ID;
    txMsg.DLC = 1;
    CAN_set_tx_msg_to_buff(&txMsg);
}
/**
* @brief	process send time
* @param	CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void can_send_heartbeat_operational(void)
{
    CanTxMsg txMsg = {0};
    txMsg.StdId = 0x704; 
      
	txMsg.Data[0] = 0x05;
    
    txMsg.IDE = CAN_STD_ID;
    txMsg.DLC = 1;
    CAN_set_tx_msg_to_buff(&txMsg);
}

/**
* @brief	move the message to transmit buffer 
* @param	CanTxMsg* TxMessage points tot tx message
* @note     none 
* @retval   none
*/
void CAN_set_tx_msg_to_buff(CanTxMsg* TxMessage)
{
	memcpy(&d_txMsg[can_tx_head++],(u8 *)TxMessage, sizeof(CanTxMsg));
    can_tx_head &= (E_TX_BUFF_SIZE - 1);
}

/**
* @brief	transmit the message in the buffer 
* @param	none
* @note     none 
* @retval   none
*/
void can_tx_msg_from_buff(void)
{
    if(can_tx_head != can_tx_tail)
    {
        CAN_Transmit(CAN1, &d_txMsg[can_tx_tail++]);
        can_tx_tail &= (E_TX_BUFF_SIZE - 1);
    }
}

/**
* @brief	send sdo reponse
* @param	s16 type: reponse type
* @param	u32 result: reponse type
* @note     none
* @retval   s16
*/
static void send_kwp_response(u32 cob_id)
{
    CanTxMsg txMsg = {0};


    kwp_cob_id = cob_id;


    txMsg.Data[0] = can_pdo_data[0];
    txMsg.Data[1] = can_pdo_data[1];
    txMsg.Data[2] = can_pdo_data[2];
    txMsg.Data[3] = can_pdo_data[3];
    txMsg.Data[4] = can_pdo_data[4];
    txMsg.Data[5] = can_pdo_data[5];
    txMsg.Data[6] = can_pdo_data[6];
    txMsg.Data[7] = can_pdo_data[7];

    txMsg.IDE = CAN_STD_ID;
    txMsg.StdId = cob_id; 
    txMsg.DLC = CAN_DATA_LEN_DEF;

    CAN_Transmit(CAN1, &txMsg);
    return;
}

/**
* @brief	process kwp message
* @param	CanRxMsg* RxMessage: buffer
* @note     none
* @retval   none
*/
static void process_kwp_mosi(CanRxMsg* RxMessage)
{
    u8 service_id;
    u32 *long_ptr = 0;
    u16 *short_ptr = 0;  
    static u8  *char_ptr = 0; 
    u8 frame_type = 0;
    u8 i;
    u8 temp_seq_no;
    u32 temp_data;
    u8 entry_option;
    u32 min_value;
    u32 max_value;
    u16 attr;
    u8 kwp_result;
    u16 data_length;    
    
    enum kwp_response_code rv;
    
    if(RxMessage == NULL) //|| CAN_PDO_Enabled == 0)
    {
        return;
    }

    can_pdo_data[0] = RxMessage->Data[0];
    can_pdo_data[1] = RxMessage->Data[1];
    can_pdo_data[2] = RxMessage->Data[2];
    can_pdo_data[3] = RxMessage->Data[3];
    can_pdo_data[4] = RxMessage->Data[4];
    can_pdo_data[5] = RxMessage->Data[5];
    can_pdo_data[6] = RxMessage->Data[6];
    can_pdo_data[7] = RxMessage->Data[7];
    
    frame_type = (can_pdo_data[0] & 0xF0) >> 4;
    
    if((can_pdo_data[1] == KWP_START_DEFAULT_SESSION) && (can_pdo_data[2] == KWP_DEFAULT_SESSION) && (can_pdo_data[0] == 2)) // stardardsession
    {
        secacc_security_state = secacc_state_default;                                      // default session
        can_pdo_data[0] = 2;
        can_pdo_data[1] = KWP_START_DEFAULT_SESSION_OK;
        can_pdo_data[2] = KWP_DEFAULT_SESSION;   // default session
        can_pdo_data[3] = 0;
        can_pdo_data[4] = 0;
        can_pdo_data[5] = 0;
        can_pdo_data[6] = 0;
        can_pdo_data[7] = 0;
        send_kwp_response(DISP_KWP_TX_ID);
        return;
    }
    kwp_multi_frame_send = 0;
    if(frame_type == KWP_SINGLE_FRM)
    {
        if(kwp_multi_frame)
        {
            kwp_multi_frame = 0;
            kwp_seq_no = 1;
            kwp_data_length = 0;
            kwp_data_index = 0;
        }        
        kwp_rx_data_length = can_pdo_data[0] & 0x0F;
        service_id = can_pdo_data[1];
        kwp_object_id = can_pdo_data[2];
        kwp_object_sub_id = can_pdo_data[3];
        switch(service_id)
        {
        case KWP_RD_ECU:
            kwp_var_index = lookup_const_object_id(kwp_ecu_params, MAX_KWP_ECU_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_FAIL;
                can_pdo_data[2] = service_id;
                can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
            }
            else
            {
                switch(kwp_ecu_params[kwp_var_index].attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    long_ptr = (u32 *) kwp_ecu_params[kwp_var_index].var;
                    can_pdo_data[0] = 5;
                    can_pdo_data[1] = RD_ECU_OK;
                    can_pdo_data[2] = (u8)((*long_ptr) & 0x000000FF);
                    can_pdo_data[3] = (u8)(((*long_ptr) & 0x0000FF00) >> 8);
                    can_pdo_data[4] = (u8)(((*long_ptr) & 0x00FF0000) >> 16);
                    can_pdo_data[5] = (u8)(((*long_ptr) & 0xFF000000) >> 24);
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;
                case 3:                          
                    char_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    can_pdo_data[0] = 4;
                    can_pdo_data[1] = RD_ECU_OK;
                    can_pdo_data[2] = char_ptr[0];
                    can_pdo_data[3] = char_ptr[1];
                    can_pdo_data[4] = char_ptr[2];
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;
                    
                    

                case TWO_BYTES_LEN:                          
                    short_ptr = (u16 *) kwp_ecu_params[kwp_var_index].var;
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = RD_ECU_OK;
                    can_pdo_data[2] = (u8)((*short_ptr) & 0x000000FF);
                    can_pdo_data[3] = (u8)(((*short_ptr) & 0x0000FF00) >> 8);
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;

                case ONE_BYTES_LEN:
                    char_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    can_pdo_data[0] = 2;
                    can_pdo_data[1] = RD_ECU_OK;
                    can_pdo_data[2] = (u8)(*char_ptr);
                    can_pdo_data[3] = 0;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;

                default:
                    char_ptr = (u8 *) kwp_ecu_params[kwp_var_index].var;
                    
                    kwp_data_length = strlen((char *)char_ptr);

                    kwp_data_length += 1;
                    kwp_seq_no = 1;
                    kwp_data_index = 0;
                    can_pdo_data[0] = (1 << 4) | ((kwp_data_length >> 8) & 0x0F);
                    can_pdo_data[1] = kwp_data_length;
                    can_pdo_data[2] = RD_ECU_OK;
                    can_pdo_data[3] = char_ptr[kwp_data_index++];
                    can_pdo_data[4] = char_ptr[kwp_data_index++];
                    can_pdo_data[5] = char_ptr[kwp_data_index++];
                    can_pdo_data[6] = char_ptr[kwp_data_index++];
                    can_pdo_data[7] = char_ptr[kwp_data_index++];
                    send_kwp_response(DISP_KWP_TX_ID);
                    kwp_data_length -= 6;
                    if(kwp_data_length > 0)
                    {
                        kwp_multi_frame = 1;
                    }
                    break; 
                }
            }
            break;
        case KWP_RD_DATA:
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_FAIL;
                can_pdo_data[2] = service_id;
                can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
            }
            else
            {
                switch(kwp_data_params[kwp_var_index].attr & VAR_LENGTH_MASK) 
                {                
                case FOUR_BYTES_LEN:

                    if(kwp_object_id == 0x6A)
                    {
                        temp_data = sys_time.t_hour;
                        TimeCurrent = sys_time.t_sec| (((u32)sys_time.t_min) << 8) | (((u32)sys_time.t_hour) << 16);
                    }
                    else if(kwp_object_id == 0x69)
                    {
                        DateCurrent = sys_time.t_date | (((u16)sys_time.t_mounth) << 5) | (((u16)sys_time.t_year) << 9);
                    }
                    long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                    can_pdo_data[0] = 6;
                    can_pdo_data[1] = KWP_RD_DATA_OK;
                    can_pdo_data[2] = kwp_object_id;
                    can_pdo_data[3] = (u8)((*long_ptr) & 0x000000FF);
                    can_pdo_data[4] = (u8)(((*long_ptr) & 0x0000FF00) >> 8);
                    can_pdo_data[5] = (u8)(((*long_ptr) & 0x00FF0000) >> 16);
                    can_pdo_data[6] = (u8)(((*long_ptr) & 0xFF000000) >> 24);
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;                
                case TWO_BYTES_LEN:   

                    
                    short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                    can_pdo_data[0] = 4;
                    can_pdo_data[1] = KWP_RD_DATA_OK;
                    can_pdo_data[2] = kwp_object_id;
                    can_pdo_data[3] = (u8)((*short_ptr) & 0x000000FF);
                    can_pdo_data[4] = (u8)(((*short_ptr) & 0x0000FF00) >> 8);
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;   
                case ONE_BYTES_LEN:
                    char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = KWP_RD_DATA_OK;
                    can_pdo_data[2] = kwp_object_id;
                    can_pdo_data[3] = (u8)(*char_ptr);
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;    
                default:
                    char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    kwp_data_length = strlen((char *)char_ptr);
                    //kwp_data_length = kwp_data_params[kwp_var_index].attr & VAR_LENGTH_MASK;
                    kwp_data_length += 2;
                    kwp_seq_no = 1;
                    kwp_data_index = 0;
                    if(kwp_data_length > 7)
                    {
                        can_pdo_data[0] = (1 << 4) | ((kwp_data_length >> 8) & 0x0F);
                        can_pdo_data[1] = kwp_data_length & 0xFF;
                        can_pdo_data[2] = KWP_RD_DATA_OK;
                        can_pdo_data[3] = kwp_object_id;
                        can_pdo_data[4] = char_ptr[kwp_data_index++];
                        can_pdo_data[5] = char_ptr[kwp_data_index++];
                        can_pdo_data[6] = char_ptr[kwp_data_index++];
                        can_pdo_data[7] = char_ptr[kwp_data_index++];
                    }
                    else
                    {
                        can_pdo_data[0] = (kwp_data_length) & 0x0F;
                        can_pdo_data[1] = KWP_RD_DATA_OK;
                        can_pdo_data[2] = kwp_object_id;
                        for(i = 3; i < kwp_data_length + 1; i++)
                        {
                           can_pdo_data[i] = char_ptr[i - 3];
                        }
                        for(i = kwp_data_length + 1; i < 8; i++)
                        {
                           can_pdo_data[i] = 0;
                        }
                    }
                    
                    send_kwp_response(DISP_KWP_TX_ID);

                    if(kwp_data_length > 6)
                    {
                        kwp_data_length -= 6;
                        kwp_multi_frame = 1;
                    }
                    break; 
                }
            }
            break;  
        case KWP_WR_DATA:  // write data
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_FAIL;
                can_pdo_data[2] = service_id;
                can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
            }
            else
            {
                if((kwp_data_params[kwp_var_index].attr & READ_WRITE) != READ_WRITE)
                {
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = KWP_FAIL;
                    can_pdo_data[2] = service_id;
                    can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;
                }         
                attr = kwp_data_params[kwp_var_index].attr;
                min_value = kwp_data_params[kwp_var_index].min;
                max_value = kwp_data_params[kwp_var_index].max;       
                switch(attr & VAR_LENGTH_MASK) 
                {
                case FOUR_BYTES_LEN:
                    temp_data = can_pdo_data[3];
                    temp_data |= (((u32)can_pdo_data[4]) << 8);
                    temp_data |= (((u32)can_pdo_data[5]) << 16);
                    temp_data |= (((u32)can_pdo_data[6]) << 24);            
                    if((((s32)temp_data < (s32)min_value || (s32)temp_data > (s32)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))
                    {
                        can_pdo_data[0] = 3;
                        can_pdo_data[1] = KWP_FAIL;
                        can_pdo_data[2] = service_id;
                        can_pdo_data[3] = KWP_OUTOF_RAGNGE;
                        can_pdo_data[4] = 0;
                        can_pdo_data[5] = 0;
                        can_pdo_data[6] = 0;
                        can_pdo_data[7] = 0;
                    }    
                    else
                    {
                        long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                        *long_ptr = temp_data;
                        can_pdo_data[0] = 2;
                        can_pdo_data[1] = KWP_WR_DATA_OK;
                        can_pdo_data[2] = kwp_object_id;
                        can_pdo_data[3] = 0;
                        can_pdo_data[4] = 0;
                        can_pdo_data[5] = 0;
                        can_pdo_data[6] = 0;
                        can_pdo_data[7] = 0;
                        if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                        {
                            kwp_needs_ee = 1;
                            kwp_ee_index = kwp_var_index;
                        }
                        if(kwp_object_id == 0x6A)
                        {
                            set_time.t_sec = TimeCurrent & 0xFF;
                            set_time.t_min = (TimeCurrent >> 8 )& 0xFF;
                            set_time.t_hour = (TimeCurrent >> 16 )& 0x1FF;
                            regulate_time_flag = RTC_SET_HOURS;
                        }
                        else if(kwp_object_id == 0x69)
                        {
                            set_time.t_date = DateCurrent & 0x1F;
                            set_time.t_mounth = (DateCurrent >> 5 )& 0xF;
                            set_time.t_year = (DateCurrent >> 9 )& 0x7F;
                            regulate_time_flag = RTC_SET_MONTH;
                        }              
                    }
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;  
                case TWO_BYTES_LEN:                          
                    temp_data = can_pdo_data[3];
                    temp_data |= (((u16)can_pdo_data[4]) << 8);
                    
                    if((((s16)temp_data < (s16)min_value || (s16)temp_data > (s16)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))    
                    {
                        can_pdo_data[0] = 3;
                        can_pdo_data[1] = KWP_FAIL;
                        can_pdo_data[2] = service_id;
                        can_pdo_data[3] = KWP_OUTOF_RAGNGE;
                        can_pdo_data[4] = 0;
                        can_pdo_data[5] = 0;
                        can_pdo_data[6] = 0;
                        can_pdo_data[7] = 0;
                    }
                    else
                    {
                        short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                        *short_ptr = temp_data;
                        can_pdo_data[0] = 2;
                        can_pdo_data[1] = KWP_WR_DATA_OK;
                        can_pdo_data[2] = kwp_object_id;
                        can_pdo_data[3] = 0;
                        can_pdo_data[4] = 0;
                        can_pdo_data[5] = 0;
                        can_pdo_data[6] = 0;
                        can_pdo_data[7] = 0;
                        if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                        {
                            kwp_needs_ee = 1;
                            kwp_ee_index = kwp_var_index;
                        }         
                    }
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;      
                    case ONE_BYTES_LEN:
                    temp_data = can_pdo_data[3];
                    
                    if((((s8)temp_data < (s8)min_value || (s8)temp_data > (s8)max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == SIGNED_VAR)) || 
                        ((temp_data < min_value || temp_data > max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK) && ((attr & SIGNED_VAR) == 0)))         
                    {
                        can_pdo_data[0] = 3;
                        can_pdo_data[1] = KWP_FAIL;
                        can_pdo_data[2] = service_id;
                        can_pdo_data[3] = KWP_OUTOF_RAGNGE;
                        can_pdo_data[4] = 0;
                        can_pdo_data[5] = 0;
                        can_pdo_data[6] = 0;
                        can_pdo_data[7] = 0;
                    }
                    else
                    {
                        char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                        *char_ptr = temp_data;
                        can_pdo_data[0] = 2;
                        can_pdo_data[1] = KWP_WR_DATA_OK;
                        can_pdo_data[2] = kwp_object_id;
                        can_pdo_data[3] = 0;
                        can_pdo_data[4] = 0;
                        can_pdo_data[5] = 0;
                        can_pdo_data[6] = 0;
                        can_pdo_data[7] = 0;
                        if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                        {
                            kwp_needs_ee = 1;
                            kwp_ee_index = kwp_var_index;
                        }
                    }
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;

                default:
                    char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                    
                    for(i = 3; i < kwp_rx_data_length + 1; i++)
                    {
                        char_ptr[i - 3] = can_pdo_data[i];
                    }

                    if((attr & VAR_LENGTH_MASK) > kwp_rx_data_length - 2)
                    {
                        char_ptr[kwp_rx_data_length - 2] = 0;
                    }
                    
                    can_pdo_data[0] = 2;
                    can_pdo_data[1] = KWP_WR_DATA_OK;
                    can_pdo_data[2] = kwp_object_id;
                    can_pdo_data[3] = 0;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                    {
                        kwp_needs_ee = 1;
                        kwp_ee_index = kwp_var_index;
                    }
                    send_kwp_response(DISP_KWP_TX_ID);
                    break; 
                }                    
            }
            break;
        case KWP_SECURITY: 
            data_length = kwp_rx_data_length - 1;
            rv = secacc_process_request(TRUE, &can_pdo_data[2], &data_length);

            if(rv == 0)
            {
                can_pdo_data[0] = data_length + 1;
                can_pdo_data[1] = KWP_SECURITY_OK;
            }
            else
            {
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_FAIL;
                can_pdo_data[2] = KWP_SECURITY;
                can_pdo_data[3] = rv;
                data_length = 2;
            }
            for(i = data_length + 2; i < 8; i++)
            {
                can_pdo_data[i] = 0;
            }
            send_kwp_response(DISP_KWP_TX_ID);
            break;    
        case KWP_ECU_RESET:
            if(kwp_object_id == 0x01) // power on reset
            {
                if(secacc_security_state >= KWP_LEVEL_2)
                {
                    can_pdo_data[0] = 2;
                    can_pdo_data[1] = KWP_ECU_RESET_OK;
                    can_pdo_data[2] = 0;   // reset status
                    can_pdo_data[3] = 0;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    reset_controller();
                }
                else
                {
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = KWP_FAIL;
                    can_pdo_data[2] = service_id;
                    can_pdo_data[3] = KWP_SECURITY_DENIED;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                }
            }
            break; 
        case startRoutineByLocalIdentifier:
            entry_option = can_pdo_data[2];
            kwp_object_id = can_pdo_data[3];

            if(entry_option == KWP_ROUTINE_RESET_DEF) // reset all the parameters to factory
            {
                default_reset = 0x3333;
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_START_ROUTINE_OK;
                can_pdo_data[2] = entry_option;
                can_pdo_data[3] = kwp_object_id;
                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
                break;
            }

            if((entry_option == KWP_START_FLASH) && (kwp_object_id == 0)) // reset all the parameters to factory
            {
                can_pdo_data[0] = 2;
                can_pdo_data[1] = KWP_START_ROUTINE_OK;
                can_pdo_data[2] = entry_option;
                can_pdo_data[3] = 0;
                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
                if(secacc_security_state == secacc_state_default)
                {
                    secacc_security_state = secacc_state_programming;
                }
//                BKP_WriteBackupRegister(BKP_DR19, KWP_FLASH_FLAG);
                deaddrop_p -> stay_in_bb_flag = KWP_FLASH_FLAG;
                osDelay(10);
                NVIC_SystemReset();
            }  

            if((entry_option == KWP_INIT01_REQ) && (kwp_object_id == 0xFB)) // emergency flash
            {
                can_pdo_data[0] = 2;
                can_pdo_data[1] = KWP_START_ROUTINE_OK;
                can_pdo_data[2] = entry_option;
                can_pdo_data[3] = 0x00;
                can_pdo_data[4] = 0x00;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);

                if(secacc_security_state == secacc_state_default)
                {
                    secacc_security_state = secacc_state_programming;
                }
//                RTC_WriteBackupRegister(RTC_BKP_DR19, KWP_FLASH_FLAG);
                deaddrop_p -> stay_in_bb_flag = KWP_FLASH_FLAG;
                osDelay(2);
                NVIC_SystemReset();
            }        
            if((entry_option == KWP_PASSIVE_MODE) && (kwp_object_id == 0x00)) // emergency flash
            {
                secacc_set_state(secacc_state_service);
                can_pdo_data[0] = 2;
                can_pdo_data[1] = KWP_START_ROUTINE_OK;
                can_pdo_data[2] = entry_option;
                can_pdo_data[3] = 0x00;
                can_pdo_data[4] = 0x00;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
                break;
            }
            
            
            kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
            if (kwp_var_index < 0)
            {
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_FAIL;
                can_pdo_data[2] = service_id;
                can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                send_kwp_response(DISP_KWP_TX_ID);
            }
            else
            {
                attr = kwp_data_params[kwp_var_index].attr;
                if((kwp_data_params[kwp_var_index].attr & READ_WRITE) != READ_WRITE)
                {
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = KWP_FAIL;
                    can_pdo_data[2] = service_id;
                    can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                    break;
                }
                
                if((attr & VAR_LENGTH_MASK) == 1)
                {
                    temp_data = *(u8 *) kwp_data_params[kwp_var_index].var;
                }
                else if((attr & VAR_LENGTH_MASK) == 2)
                {
                    temp_data = *(u16 *) kwp_data_params[kwp_var_index].var;
                }
                else if((attr & VAR_LENGTH_MASK) == 4)
                {
                    temp_data = *(u32 *) kwp_data_params[kwp_var_index].var;
                }
                min_value = kwp_data_params[kwp_var_index].min;
                max_value = kwp_data_params[kwp_var_index].max;

                switch(entry_option)
                {
                case KWP_ROUTINE_INC_1:
                    if((temp_data + 1 <= max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK))
                    {
                        temp_data++;
                        kwp_result = 1;
                    }
                    else
                    {
                        kwp_result = 0;
                    }
                    break;
                case KWP_ROUTINE_INC_10:
                    if((temp_data + 10 <= max_value) && ((attr & LIMIT_CHK) == LIMIT_CHK))
                    {
                        temp_data += 10;
                        kwp_result = 1;
                    }
                    else
                    {
                        kwp_result = 0;
                    }
                    break;
                case KWP_ROUTINE_DEC_1:
                    if((temp_data >= min_value + 1) && ((attr & LIMIT_CHK) == LIMIT_CHK))
                    {
                        temp_data--;
                        kwp_result = 1;
                    }
                    else
                    {
                        kwp_result = 0;
                    }
                    break;
                case KWP_ROUTINE_DEC_10:
                    if((temp_data >= min_value + 10) && ((attr & LIMIT_CHK) == LIMIT_CHK))
                    {
                        temp_data -= 10;
                        kwp_result = 1;
                    }
                    else
                    {
                        kwp_result = 0;
                    }
                    break;
                case KWP_ROUTINE_SET_DEF:
                    temp_data = kwp_data_params[kwp_var_index].default_value;
                    kwp_result = 1;
                    break;
                default:
                    break;
                }  
                if(kwp_result == 1)
                {     
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = KWP_START_ROUTINE_OK;
                    can_pdo_data[2] = entry_option;
                    can_pdo_data[3] = kwp_object_id;
                    if(attr & VAR_CLR_SCREEN)
                    {
                        clearScreen = 1;
                    }
                    if((attr & VAR_LENGTH_MASK) == 1)
                    {
                        *(u8 *) kwp_data_params[kwp_var_index].var = temp_data;
                    }
                    else if((attr & VAR_LENGTH_MASK) == 2)
                    {
                        *(u16 *) kwp_data_params[kwp_var_index].var = temp_data;
                    }
                    else if((attr & VAR_LENGTH_MASK) == 4)
                    {
                        *(u32 *) kwp_data_params[kwp_var_index].var = temp_data;
                    }
                }
                else
                {
                    can_pdo_data[0] = 3;
                    can_pdo_data[1] = KWP_FAIL;
                    can_pdo_data[2] = service_id;
                    can_pdo_data[3] = KWP_REQUEST_OUT_OF_RANGE;
                }

                can_pdo_data[4] = 0;
                can_pdo_data[5] = 0;
                can_pdo_data[6] = 0;
                can_pdo_data[7] = 0;
                if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                {
                    kwp_needs_ee = 1;
                    kwp_ee_index = kwp_var_index;
                }

                send_kwp_response(DISP_KWP_TX_ID);
            }
            break;     
        case KWP_RD_DTC:
            if(kwp_object_id == 0x01) // read all active code
            {
                if(display_active_fault_index == 1) // first 1 top priority
                {
                    can_pdo_data[0] = 2 + 2;
                    can_pdo_data[1] = KWP_RD_DTC_OK;
                    can_pdo_data[2] = 1;
                    can_pdo_data[3] = display_active_fault_code[0] & 0xFF;
                    can_pdo_data[4] = (display_active_fault_code[0] >> 8) & 0xFF;
                }
                else if(display_active_fault_index == 2)//  2 top priority fault
                {
                    can_pdo_data[0] = 2 + (2 * 2);
                    can_pdo_data[1] = KWP_RD_DTC_OK;
                    can_pdo_data[2] = 2;
                    for(i = 0; i < 2; i++)
                    {
                        can_pdo_data[3 + 2*i] = display_active_fault_code[i] & 0xFF;
                        can_pdo_data[4 + 2*i] = (display_active_fault_code[i] >> 8) & 0xFF;
                    }
                }
                else
                {
                    can_pdo_data[0] = 2 ;
                    can_pdo_data[1] = KWP_RD_DTC_OK;
                    can_pdo_data[2] = 0;
                    can_pdo_data[3] = 0;
                    can_pdo_data[4] = 0;

                
                }
                send_kwp_response(DISP_KWP_TX_ID);
            }
            
            break;      
        case KWP_RD_SDTC:
            if(kwp_object_id == 0x02 || kwp_object_id == 0x03) // read all active code
            {
                CalTotalStoredFault(display_stored_fault, &display_total_stored_fault);

                if(display_total_stored_fault > 0) // first 1 top priority
                {
                    FillSDTCTxBuf(display_stored_fault,display_total_stored_fault, 0x04);
                    kwp_data_index = 0;
                    can_pdo_data[0] = 0x10 | (((display_total_stored_fault * 18 + 2) >> 8) & 0x0F);
                    can_pdo_data[1] = ((display_total_stored_fault * 18 + 2) & 0xff);  // fault_code 2 bytes, truck work time 4 bytes, component work time 4 bytes, date time 4 bytes, length1 1bytes, length2 1 bytes, extra data 2 bytes, 
                    kwp_data_length = (display_total_stored_fault * 18 + 2);
                    can_pdo_data[2] = KWP_RD_SDTC_OK;
                    can_pdo_data[3] = display_total_stored_fault;
                    can_pdo_data[4] = dtc_error_tx_buf[kwp_data_index++];
                    can_pdo_data[5] = dtc_error_tx_buf[kwp_data_index++];
                    can_pdo_data[6] = dtc_error_tx_buf[kwp_data_index++];
                    can_pdo_data[7] = dtc_error_tx_buf[kwp_data_index++];
                    kwp_seq_no = 1;
                    kwp_multi_frame = 1;
                    kwp_data_length -= 6;
                    char_ptr = dtc_error_tx_buf;
                }
                else
                {
                     can_pdo_data[0] = 2;
                     can_pdo_data[1] = KWP_RD_SDTC_OK;
                     can_pdo_data[2] = 0;
                }

                send_kwp_response(DISP_KWP_TX_ID);
            }
            
            break;     
        case KWP_CLR_DTC:
            if(kwp_object_id == 0x02 || kwp_object_id == 0x03) // read all active code
            {
                error_reset = VAR_STORED_FAULT0_INDEX;
                can_pdo_data[0] = 2;
                can_pdo_data[1] = KWP_CLR_DTC_OK;
                can_pdo_data[2] = kwp_object_id;
                send_kwp_response(DISP_KWP_TX_ID);
            }
            break;   
        case KWP_RD_DATA_BY_CMD_ID:
            if(kwp_object_id == 0x10 && kwp_object_sub_id == 0x00) // read all active code
            {
                can_pdo_data[0] = 7;
                can_pdo_data[1] = KWP_RD_DATA_BY_CMD_ID_OK;
                can_pdo_data[2] = kwp_object_id;
                can_pdo_data[3] = kwp_object_sub_id;
                can_pdo_data[4] = truck_id & 0xFF;
                can_pdo_data[5] = (truck_id >> 8) & 0xFF;
                can_pdo_data[6] = (truck_id >> 16) & 0xFF;
                can_pdo_data[7] = (truck_id >> 24) & 0xFF;
                
                send_kwp_response(DISP_KWP_TX_ID);
            }
            break;             
        default: break;
        }            
    }
    else if(frame_type == KWP_FLOW_CONTROL_FRM)  // tx data more than 4 bytes
    {
        //kwp_continue_to_send = sdo_data[0] & 0x0F;
        kwp_blk_size = can_pdo_data[1];
        kwp_multi_frame_timeout = can_pdo_data[2];
        mulit_char_ptr = char_ptr;
        kwp_multi_frame_send = 1;
    }
    else    // other frame
    {
        if(frame_type == KWP_FIRSET_FRAME)  // write data more than 4 bytes
        {
            kwp_rx_data_length = ((((s16)can_pdo_data[0]) & 0x0F) << 8) | can_pdo_data[1];
            service_id = can_pdo_data[2];
            kwp_object_id = can_pdo_data[3];
            switch(service_id)
            {
            case KWP_WR_DATA:
                kwp_var_index = lookup_object_id(kwp_data_params, MAX_KWP_DATA_OBJECT_DEF, kwp_object_id);
                if (kwp_var_index < 0 )
                {
                    can_pdo_data[0] = 2;
                    can_pdo_data[1] = KWP_FAIL;
                    can_pdo_data[2] = service_id;
                    can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                    can_pdo_data[4] = 0;
                    can_pdo_data[5] = 0;
                    can_pdo_data[6] = 0;
                    can_pdo_data[7] = 0;
                    send_kwp_response(DISP_KWP_TX_ID);
                }
                else
                {
                    switch(kwp_data_params[kwp_var_index].attr & VAR_LENGTH_MASK) 
                    {
                    case FOUR_BYTES_LEN:
                        temp_data = can_pdo_data[3];
                        temp_data |= (((u32)can_pdo_data[4]) << 8);
                        temp_data |= (((u32)can_pdo_data[5]) << 16);
                        temp_data |= (((u32)can_pdo_data[6]) << 24);

                        if((temp_data < kwp_data_params[kwp_var_index].min || temp_data > kwp_data_params[kwp_var_index].max) && ((kwp_data_params[kwp_var_index].attr & LIMIT_CHK) == LIMIT_CHK))
                        {
                            can_pdo_data[0] = 3;
                            can_pdo_data[1] = KWP_FAIL;
                            can_pdo_data[2] = service_id;
                            can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                            can_pdo_data[4] = 0;
                            can_pdo_data[5] = 0;
                            can_pdo_data[6] = 0;
                            can_pdo_data[7] = 0;
                        }
                        else
                        {
                            long_ptr = (u32 *) kwp_data_params[kwp_var_index].var;
                            *long_ptr = temp_data;
                            can_pdo_data[0] = 2;
                            can_pdo_data[1] = KWP_WR_DATA_OK;
                            can_pdo_data[2] = kwp_object_id;
                            can_pdo_data[3] = 0;
                            can_pdo_data[4] = 0;
                            can_pdo_data[5] = 0;
                            can_pdo_data[6] = 0;
                            can_pdo_data[7] = 0;
                            if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                            {
                                kwp_needs_ee = 1;
                                kwp_ee_index = kwp_var_index;
                            }
                        }
                        send_kwp_response(DISP_KWP_TX_ID);
                        break;

                    case TWO_BYTES_LEN:                          
                        temp_data = can_pdo_data[3];
                        temp_data |= (((u16)can_pdo_data[4]) << 8);
                        
                        if((temp_data < kwp_data_params[kwp_var_index].min || temp_data > kwp_data_params[kwp_var_index].max) && ((kwp_data_params[kwp_var_index].attr & LIMIT_CHK) == LIMIT_CHK))
                        {
                            can_pdo_data[0] = 3;
                            can_pdo_data[1] = KWP_FAIL;
                            can_pdo_data[2] = service_id;
                            can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                            can_pdo_data[4] = 0;
                            can_pdo_data[5] = 0;
                            can_pdo_data[6] = 0;
                            can_pdo_data[7] = 0;
                        }
                        else
                        {
                            short_ptr = (u16 *) kwp_data_params[kwp_var_index].var;
                            *short_ptr = temp_data;
                            can_pdo_data[0] = 2;
                            can_pdo_data[1] = KWP_WR_DATA_OK;
                            can_pdo_data[2] = kwp_object_id;
                            can_pdo_data[3] = 0;
                            can_pdo_data[4] = 0;
                            can_pdo_data[5] = 0;
                            can_pdo_data[6] = 0;
                            can_pdo_data[7] = 0;
                            if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                            {
                                kwp_needs_ee = 1;
                                kwp_ee_index = kwp_var_index;
                            }
                        }
                        send_kwp_response(DISP_KWP_TX_ID);
                        break;
                    case ONE_BYTES_LEN:
                        temp_data = can_pdo_data[3];
                        if((temp_data < kwp_data_params[kwp_var_index].min || temp_data > kwp_data_params[kwp_var_index].max) && ((kwp_data_params[kwp_var_index].attr & LIMIT_CHK) == LIMIT_CHK))
                        {
                            can_pdo_data[0] = 3;
                            can_pdo_data[1] = KWP_FAIL;
                            can_pdo_data[2] = service_id;
                            can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                            can_pdo_data[4] = 0;
                            can_pdo_data[5] = 0;
                            can_pdo_data[6] = 0;
                            can_pdo_data[7] = 0;
                        }
                        else
                        {
                            char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                            *char_ptr = temp_data;
                            can_pdo_data[0] = 2;
                            can_pdo_data[1] = KWP_WR_DATA_OK;
                            can_pdo_data[2] = kwp_object_id;
                            can_pdo_data[3] = 0;
                            can_pdo_data[4] = 0;
                            can_pdo_data[5] = 0;
                            can_pdo_data[6] = 0;
                            can_pdo_data[7] = 0;
                            if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                            {
                                kwp_needs_ee = 1;
                                kwp_ee_index = kwp_var_index;
                            }
                        }
                        send_kwp_response(DISP_KWP_TX_ID);
                        break;

                    default:
                        char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;
                        kwp_data_length = kwp_data_params[kwp_var_index].attr & VAR_LENGTH_MASK;
                        kwp_seq_no = 1;
                        kwp_data_index = 0;
                        kwp_blk_size = 0x00;
                        
                        char_ptr[kwp_data_index++] = can_pdo_data[4];
                        char_ptr[kwp_data_index++] = can_pdo_data[5];
                        char_ptr[kwp_data_index++] = can_pdo_data[6];
                        char_ptr[kwp_data_index++] = can_pdo_data[7];
                         
                        can_pdo_data[0] = (KWP_FLOW_CONTROL_FRM << 4) | CONTINUE_TO_SEND;
                        can_pdo_data[1] = kwp_blk_size;
                        can_pdo_data[2] = KWP_MIN_INTERVAL;
                        can_pdo_data[3] = 0xFF;
                        can_pdo_data[4] = 0xFF;
                        can_pdo_data[5] = 0xFF;
                        can_pdo_data[6] = 0xFF;
                        can_pdo_data[7] = 0xFF;
                        send_kwp_response(DISP_KWP_TX_ID);
                        kwp_rx_data_length -= 6;
                        break; 
                    }
                }
                break;

            default:
                break;
            } 
        }
        else if(frame_type == CONSECUTIVE_FRM ) // write data more than 4 bytes
        {
            temp_seq_no = can_pdo_data[0] & 0x0F;
            
            if(temp_seq_no != kwp_seq_no)
            {
                can_pdo_data[0] = 3;
                can_pdo_data[1] = KWP_FAIL;
                can_pdo_data[2] = service_id;
                can_pdo_data[3] = KWP_FUN_NOT_SUPPORT;
                can_pdo_data[4] = 0xFF;
                can_pdo_data[5] = 0xFF;
                can_pdo_data[6] = 0xFF;
                can_pdo_data[7] = 0xFF;
                send_kwp_response(DISP_KWP_TX_ID);
                return;
            }
            char_ptr = (u8 *) kwp_data_params[kwp_var_index].var;

            if(kwp_rx_data_length > 7)
            {
                for(i = 1; i < 8; i++)
                {
                    char_ptr[kwp_data_index++] = can_pdo_data[i];
                }
                kwp_rx_data_length -= 7;
            }
            else
            {
                for(i = 1; i < kwp_rx_data_length + 1; i++)
                {
                    char_ptr[kwp_data_index++] = can_pdo_data[i];
                }
                char_ptr[kwp_data_index++] = 0x00; // end character
                kwp_rx_data_length = 0;
                if(kwp_data_params[kwp_var_index].attr & SAVE_TO_EE)
                {
                    kwp_needs_ee = 1;
                    kwp_ee_index = kwp_var_index;
                }
                can_pdo_data[0] = 2;
                can_pdo_data[1] = KWP_WR_DATA_OK;
                can_pdo_data[2] = kwp_object_id;
                can_pdo_data[3] = 0xFF;
                can_pdo_data[4] = 0xFF;
                can_pdo_data[5] = 0xFF;
                can_pdo_data[6] = 0xFF;
                can_pdo_data[7] = 0xFF;
                send_kwp_response(DISP_KWP_TX_ID);
            }
            kwp_seq_no++;
        } 
    }
    return;
}

/**
* @brief	Get total stored fault
* @param	union STORED_FAULT_ST *stored_fault points to stored fault 
* @param	u16 *total_fault points to total stored fault
* @note     none 
* @retval   none
*/
void CalTotalStoredFault(E_STORED_FAULT_PARAM *stored_fault, u16 *total_fault)
{
    u8 i = 0;
   
    (*total_fault) = 0;
    if((stored_fault[MAX_STORED_FAULT - 1]).code != 0)
    {
        (*total_fault) = MAX_STORED_FAULT;
    }
    else
    {
        for(i = 0; i < MAX_STORED_FAULT; i++)
        {
            if((stored_fault[i]).code != 0)
            {
                (*total_fault)++;
            }
            else
            {
                break;
            }
        }
    }
}

void FillSDTCTxBuf(E_STORED_FAULT_PARAM *stored_fault, u16 total_fault, u16 device)
{
    int i, j, k;
    u16 year;
    u32 temp_data; // convert to seconds
    u16 code;
    
    j = 0;


    for(i = 0; i < total_fault; i++)
    {
        //fault code

        code = stored_fault[i].code;

        if(device == KWP_GATEWAY_LIFT_ADDR || device == KWP_GATEWAY_TRAC_ADDR)// traction or pump
        {
            code += ZAPI_ERR_CODE_OFFSET;
        }
        
        dtc_error_tx_buf[j++] = code & 0xFF;
        dtc_error_tx_buf[j++] = (code >> 8) & 0xFF;


        temp_data = stored_fault[i].hourmeter * 360; // convert to seconds        

        // truck work time
        dtc_error_tx_buf[j++] = (temp_data) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 8) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 16) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 24) & 0xFF;


        // component work time
        dtc_error_tx_buf[j++] = (temp_data) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 8) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 16) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 24) & 0xFF;
        // Date time

        temp_data = 0;
        year = 2000 + stored_fault[i].year;

        for(k = 1970; k < year; k++)
        {
            if((((k % 4) == 0) && ((k % 100) != 0)) || ((k % 400) == 0)) // leap year
                temp_data += 366;
            else
                temp_data += 365;
        }

        for(k = 1; k < stored_fault[i].month; k++)
        {
            if(k == 4 || k == 6 || k == 9 || k == 11)
            {
                temp_data += 30;
            }
            else if( k == 2)
            {
                if(((year % 4 == 0) && (year % 100 != 0)) || ((year % 400) == 0)) // leap year
                    temp_data += 29;
                else
                    temp_data += 28;
            }
            else
            {
                temp_data += 31;
            }
        }

        temp_data += stored_fault[i].day - 1;
        temp_data *= 24;
        
        temp_data += stored_fault[i].hour;
        temp_data *= 60;

        temp_data += stored_fault[i].minutes;
        temp_data *= 60;
        
        temp_data += stored_fault[i].sec;

        
        // date time
        dtc_error_tx_buf[j++] = (temp_data) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 8) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 16) & 0xFF;
        dtc_error_tx_buf[j++] = (temp_data >> 24) & 0xFF;
        
        dtc_error_tx_buf[j++] = 0x01; // len 1
        dtc_error_tx_buf[j++] = 0x00; // len 2
        //fault code
        dtc_error_tx_buf[j++] = code & 0xFF;
        dtc_error_tx_buf[j++] = (code >> 8) & 0xFF;
    }
    
}

void secacc_set_state( enum secacc_state state )
{
   secacc_security_state = state;
}


/** Calculate Key
*
*  \param seed Input value.
*  \param state Kind of algorithm.
*
*  \return Calculated Key
*/
static u32 secacc_calculate_key( u32 seed, enum secacc_state state )
{
   switch(state)
   {
      case secacc_state_programming: return secacc_crypto_12( seed );
      case secacc_state_service    : return secacc_crypto_34( seed );
      case secacc_state_eopl       : return secacc_crypto_56( seed );
      default                      : return 0; /* will never happen */
   }
}

/** Copy 32Bit word into 4 Byte array.
*
*  \param dat Pointer to the destination.
*  \param value To by copied.
*/
static void secacc_u32_copy( u8 *dat, u32 value )
{
   dat[3] = (u8)(value >> 24);
   dat[2] = (u8)(value >> 16);
   dat[1] = (u8)(value >> 8);
   dat[0] = (u8)value;
}
static enum kwp_response_code secacc_seed( u8 condition, u8  *dat, u16 *len, enum secacc_state state)
{
   enum kwp_response_code rv;
   u32 seed;
   if (*len != 1) /* Wrong Length */
   {
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      if( FALSE == condition )
      {
         rv = KWP_RESPONSE_CODE_CONDITIONSNOTCORRECT;
      }
      else
      {
         if( state == secacc_security_state )
         {
            /* ECU already unlocked */
            dat[1] = 0;
            dat[2] = 0;
            *len   = 3;
            rv = KWP_RESPONSE_CODE_NONE;
         }
         else
         {
          seed = secacc_fortuna();
          secacc_code = secacc_calculate_key( seed, state );
          secacc_expected_service = dat[0]+1;
          *len = 5;
          secacc_u32_copy( &dat[1], seed );
          rv = KWP_RESPONSE_CODE_NONE;
         }
      }
   }
   return rv;
   
}
/** Key Calculation for Security Access 1/2.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_12( u32 seed )
{
   /* calculate key */
   const u32 a = 0x974c58ab;
   const u32 b = 0xfedcba98;
   const u32 c = 0x98765432;
   u32 x = seed + a;

   if( (0xffffffff - a) < seed)
   {
      x ^= b;
   }
   else
   {
      x ^= c;
   }

   x = (x >> 11) | ((x & 0x7ff) << 21);

   return x;
}

/** Key Calculation for Security Access 3/4.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_34( u32 seed )
{
   return seed + 0x1AF7;
}

/** Key Calculation for Security Access 5/6.
*
*  \param seed Input value.
*
*  \return Calculated Key.
*/
static u32 secacc_crypto_56( u32 seed )
{
   u32 rv;
   u8 data[8];

   data[3] = (u8)seed;
   seed >>= 8;
   data[2] = (u8)seed;
   seed >>= 8;
   data[1] = (u8)seed;
   seed >>= 8;
   data[0] = (u8)seed;

   data[0] += 0x97;
   data[1] += 0x28;
   data[2] += 0xAA;
   data[3] += 0x55;

   rv = data[3];
   rv <<= 8;
   rv |= data[2];
   rv <<= 8;
   rv |= data[1];
   rv <<= 8;
   rv |= data[0];

   return rv;
}
u16 fortune_u16( struct fortune *fate )
{
   if( NULL == fate)
   {
      fate = &fortune_fate;
   }
   fate->random = fate->random * 1103515245 + 12345;
   return (u16)(fate->random/65536);
}

void fortune_set(struct fortune *fate, void *dest, u32 size )
{
   u8 *d=dest;
   while(size--)
   {
      *d++=(u8) fortune_u16(fate);
   }
}
u32 fortune_u32( struct fortune *fate )
{
    u32 rv;
    fortune_set(fate, &rv, sizeof(rv));
    return rv;
}
static void fortune_initialize(struct fortune *fate, u32 value)
{
   if( NULL == fate)
   {
      fate = &fortune_fate;
   }
   fate->random=value;
}
static u32 secacc_fortuna( void )
{
   struct fortune fortune_obj;
   fortune_initialize( &fortune_obj, GetSystemTime());
   return fortune_u32( &fortune_obj );
}

/**  Read 32 Bit word from given address.
*
* \param dat Source Pointer
*
* \return 32Bit value
*/
static u32 secacc_u32(u8 *dat )
{
   return ((u32)dat[3]<<24)+((u32)dat[2]<<16)+((u32)dat[1]<<8)+dat[0];
}

/** Process Key Request */
static enum kwp_response_code secacc_key( u8 *dat, u16 *len, enum secacc_state state)
{
   enum kwp_response_code rv;

   if (*len != 5)
   {  /* Invalid length */
      rv = KWP_RESPONSE_CODE_INVALIDFORMAT;
   }
   else
   {
      if ( secacc_expected_service == dat[0])
      {
         /* Securitysecacc_state_access in progress */
         if( secacc_u32(&dat[1]) == secacc_code  )
         {  /* valid key */
            secacc_security_state = state;
            *len = 1;
            rv = KWP_RESPONSE_CODE_NONE;
         }
         else
         {  /* invalid key */
            rv = KWP_RESPONSE_CODE_INVALIDKEY;
         }
      }
      else
      {  /* request sequence error */
         rv = KWP_RESPONSE_CODE_REQUESTSEQUENCEERROR;
      }
   }
   return rv;
}

/**  Read 32 Bit word from given address.
*
* \param dat Source Pointer
*
* \return 32Bit value
*/

enum kwp_response_code secacc_process_request( u8 condition, u8  *dat, u16 *len)
{
   switch( dat[0] )
   {
#if SECACC_12
      case 0x01:  return secacc_seed( condition, dat, len, secacc_state_programming );
      case 0x02:  return secacc_key ( dat, len, secacc_state_programming );
#endif

#if SECACC_34
      case 0x03:  return secacc_seed( TRUE,  dat, len, secacc_state_service);
      case 0x04:  return secacc_key ( dat, len, secacc_state_service );
#endif

#if SECACC_56
      case 0x05:  return secacc_seed( TRUE, dat, len, secacc_state_eopl);
      case 0x06:  return secacc_key ( dat, len, secacc_state_eopl );
#endif
      default:    return KWP_RESPONSE_CODE_SUBFUNCTIONNOTSUPPORTED;
   }
}
static void process_sync_msg(CanRxMsg* RxMessage)
{
  
}

void kwp_period_transmit(void)
{
    s16 i;

    if(kwp_multi_frame_send == 0)
    {
        return;
    }

    if(kwp_multi_frame_timeout >= 0)
    {
        kwp_multi_frame_timeout -= 4;
        return;
    }
    if(kwp_multi_frame)
    {
        can_pdo_data[0] = (CONSECUTIVE_FRM << 4) | kwp_seq_no++;
        kwp_seq_no &= 0x0F;
        
        if(kwp_data_length >= 7)
        {
            for(i = 1; i < 8; i++)
            {
                can_pdo_data[i] = mulit_char_ptr[kwp_data_index++];
            }
            kwp_data_length -= 7;
            send_kwp_response(kwp_cob_id);

        }
        else if(kwp_data_length > 0)
        {
            for(i = 1; i < kwp_data_length + 1; i++)
            {
                can_pdo_data[i] = mulit_char_ptr[kwp_data_index++];
            }
            for(i = kwp_data_length + 1; i < 8; i++)
            {
                 can_pdo_data[i] = 0;
            }
            kwp_data_length = 0;
            send_kwp_response(kwp_cob_id);
        }
        if(kwp_data_length == 0)
        {
            kwp_seq_no = 1; 
            kwp_data_index = 0;
            kwp_multi_frame = 0;
            kwp_multi_frame_timeout = 0;
        }
    }
}

/**
* @brief	process CAN message
* @param	CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void process_msg(CanRxMsg* RxMessage)
{
    u32 id = RxMessage->StdId;
    u16 msgType = id & 0x7FF;
//    u8 kwpPhysicalAddr = 0;

    if(RxMessage->IDE == CAN_Id_Extended) // Skip extended message
    {
        return;
    }
    
    switch(msgType) 
    {
    case NMT:
        process_canopen_nmt(RxMessage);
        break;
    case DISP_KWP_RX_ID:
    case DISP_KWP_RX_ID_ALL:
        process_kwp_mosi(RxMessage);
        break;
	case BR1189_LIFT_PDO1_RX_ID:
		process_br1189_lift_pdo1_rx(RxMessage);
	    break;
	case BR1189_LIFT_PDO3_RX_ID:
		process_br1189_lift_pdo3_rx(RxMessage);
	    break;
	case BR1189_TRAC_PDO1_RX_ID:
		process_br1189_trac_pdo1_rx(RxMessage);
	    break;
	case BR1189_TRAC_PDO3_RX_ID:
		process_br1189_trac_pdo3_rx(RxMessage);
	    break;
    case BR1189_TRAC_PDO5_RX_ID1:
    case BR1189_TRAC_PDO5_RX_ID2:  
        process_br1189_trac_pdo5_rx(RxMessage);
        break;
	case BR1189_STEER_PDO1_RX_ID:
		process_br1189_steer_pdo1_rx(RxMessage);
	    break;
	case BR1189_STEER_PDO3_RX_ID:
		process_br1189_steer_pdo3_rx(RxMessage);
	    break;
    case SYNC_MSG_PDO_RX: 
        process_sync_msg(RxMessage); 
        break;  
    default:break;
    }
    return;
}
/***********************************************test function********************************************/
/**
* @brief	process send time
* @param	CanRxMsg* RxMessage: points to rx message
* @note     none
* @retval   none
*/
void can_send_testtamp(U8 *databuf)
{
    CanTxMsg txMsg = {0};
    txMsg.StdId = 0x181; 
      
	txMsg.Data[0] = databuf[0];
    txMsg.Data[1] = databuf[1];
    txMsg.Data[2] = databuf[2];
    txMsg.Data[3] = databuf[3];
    txMsg.Data[4] = databuf[4];
    txMsg.Data[5] = databuf[5];
    txMsg.Data[6] = databuf[6];
    txMsg.Data[7] = databuf[7];
    
    txMsg.IDE = CAN_STD_ID;
    txMsg.DLC = 8;
    CAN_set_tx_msg_to_buff(&txMsg);
}
